/**
 * The Woosta Table Football Engine
 *
 * License... etc.
 **
 * Physics Engine - External bindings for Bullet
 **
 * Author: Sean Micklethwaite
 **/

#ifndef __physics_ext_bulletH__
#define __physics_ext_bulletH__

#include "dynamic.h"
#include "renderable.h"
#include "3d_maths.h"

#include <bullet/LinearMath/btScalar.h>
#include <bullet/btBulletDynamicsCommon.h>

/// Maximum number of collisions before treated as contact.
#define INTERACTION_THRESHOLD	8

#define TOVECT(v)	tVector((v).x(), (v).y(), (v).z())
#define FROMVECT(v)	btVector3((v).x(), (v).y(), (v).z())
#define TOORI(m)	Orientation3D(TOVECT((m)[0]), TOVECT((m)[1]), TOVECT((m)[2]))
#define FROMORI(m)	btMatrix3x3((m)[0][0], (m)[0][1], (m)[0][2], \
	(m)[1][0], (m)[1][1], (m)[1][2], (m)[2][0], (m)[2][1], (m)[2][2])

namespace WtfEngine 
{
	/**
	 * Interface class to allow Bullet to access our meshes, for collision detection.
	 * TODO: Make it work.
	 **/
	/*class MeshInterface : public btStridingMeshInterface {
		WtfGraphics::Mesh::tRef	mrMesh;
		iVector2D				mIndex;
		
		public:
			MeshInterface(const WtfGraphics::Mesh::tRef& rMesh);
			
			virtual void	getLockedVertexIndexBase(
					unsigned char **vertexbase,
	 				int& numverts, PHY_ScalarType& type, int& stride,
	  				unsigned char **indexbase,int & indexstride,int& numfaces,
	   				PHY_ScalarType& indicestype,int subpart=0);
		
			virtual void	getLockedReadOnlyVertexIndexBase(
					const unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& stride,
	 				const unsigned char **indexbase,int & indexstride,int& numfaces,
	  				PHY_ScalarType& indicestype,int subpart=0) const;
	
			virtual void	unLockVertexBase(int subpart);

			virtual void	unLockReadOnlyVertexBase(int subpart) const;

			virtual int		getNumSubParts() const;

			virtual void	preallocateVertices(int numverts);
			virtual void	preallocateIndices(int numindices);
	}; */
	
	class PhysicalModel : public DataObject
	{
		btCollisionShape *	mpCollisionShape;
		tScalar	mMassQuot;

		btVector3 mLocalInertia;
		
		public:
			PhysicalModel(const StringPool::Entry& sName, btCollisionShape * sh, tScalar mq);
			virtual ~PhysicalModel();

			tScalar getMassQuot() const{
				return mMassQuot;
			};
		
			/**** Bullet specific ****/
			btCollisionShape * getCollisionShape() const {
				return mpCollisionShape;
			};

			const btVector3& getLocalInertia() const {
				return mLocalInertia;
			};
			void setLocalInertia(const btVector3& v) {
				mLocalInertia = v;
			};

			DATA_OBJECT(PhysicalModel, GC_ADDEDSIZE_AUTO(DataObject, PhysicalModel));
	};

	/**** Forward Declarations ****/

	class Static;
	class Physical;
	class PhysicalGroup;
	class Force;
	class CollisionHeap;

	
	class ObjectConnectionEvent : public ObjectInteractionEvent
	{
		tPosition	vPosition;
		
		public:
			ObjectConnectionEvent(GameObject *pConnector, GameObject *pConnectee, const tPosition& pos)
			: ObjectInteractionEvent(pConnector, pConnectee), vPosition(pos){
			};
		
			const tPosition& getPosition(){
				return vPosition;
			};
	};
	
	class ObjectDisconnectionEvent : public ObjectInteractionEvent
	{
		public:
			ObjectDisconnectionEvent(GameObject *pConnector, GameObject *pConnectee)
			: ObjectInteractionEvent(pConnector, pConnectee){
			};
	};
	
	
	/**
	 * A class representing a group of connected physical & static objects,
	 * each connected in a pairwise fashion via a 'Connection'.
	 * The system is resolved before object update, in order to balance
	 * the internal forces based upon the external forces acting upon the
	 * system.
	 *
	 * This class provides the basis for all rigid body dynamics.
	 **/
	class PhysicalSystem : public GcObject
	{
		virtual ~PhysicalSystem() {};
	};
	
	
	class WorldObject : public Renderable
	{
	protected:
		PhysicalModel::tRef      mModel;
		PhysicalModel::tCacheRef mrModel;
		btDefaultMotionState     mState;
		btRigidBody *            mpBody;

	public:
		WorldObject(const tCreationOptions& args,
					const WtfGraphics::GraphicsData::tCacheRef& d,
					const PhysicalModel::tCacheRef& m, const btRigidBody::btRigidBodyConstructionInfo& info);
		virtual ~WorldObject();
		
		// Override default Render()
		virtual void Render();
		virtual void Destroy();

		void RenderStart();
		void RenderEnd();
		/// This is the rendering which occurs in the object's local coordinates
		virtual void RenderLocal();
		
		void setPosition(tVector pos) {
			btTransform t = getBody()->getCenterOfMassTransform();
			t.setOrigin(FROMVECT(pos));
			getBody()->setCenterOfMassTransform(t);
			mState.setWorldTransform(t);
		}
		void setOrientation(Orientation3D ori) {
			btTransform t = getBody()->getCenterOfMassTransform();
			t.setBasis(FROMORI(ori));
			getBody()->setCenterOfMassTransform(t);
			mState.setWorldTransform(t);
		}

		tVector getPosition() const {
			btTransform t;
			mState.getWorldTransform(t);
			return TOVECT(t.getOrigin());
		}
		Orientation3D getOrientation() const {
			btTransform t;
			mState.getWorldTransform(t);
			return TOORI(t.getBasis());
		}
		
		tVector getRotation() const {
			// TODO
			return tVector();
		}
		void setRotation(tVector rot) {
			btTransform t;
			mState.getWorldTransform(t);
			// yaw = z, pitch = y, roll = x
			t.setRotation(btQuaternion(rot[2], rot[1], rot[0]));
			getBody()->setCenterOfMassTransform(t);
			mState.setWorldTransform(t);
		}
		
		btRigidBody * getBody() const {
			return mpBody;
		};
		
		btMotionState * getMotionState() {
			return &mState;
		};
		

		virtual unsigned CalculateSerializedSize();
		virtual unsigned Serialize(void * pBuf);
		virtual unsigned Deserialize(void * pBuf);

		GC_OBJECT(WorldObject, sizeof(WorldObject) + sizeof(btRigidBody));
	};
	
	class Physical : public WorldObject
	{
		PhysicalModel::tRef	mModel;
		
		public:
			typedef GcReference<Physical>	tRef;

			struct Force : public GcObject, public tVector
			{
				typedef GcReference<Force>	tRef;

				tVector		relativePosition;
				tGameTime	remainingTime;

				Force()
					: tVector(), relativePosition() {};
				Force(const tVector& f)
					: tVector(f), relativePosition() {};
				Force(const tVector& f, const tVector& p)
					: tVector(f), relativePosition(p) {};
			};
			
			typedef struct PhysicalCreationOptions : public WorldObject::tCreationOptions {

			} tCreationOptions;

			Physical(const tCreationOptions& args,
					 const WtfGraphics::GraphicsData::tCacheRef& d,
					 const PhysicalModel::tCacheRef& m, bool isParticle = false);
			virtual ~Physical();

			void applyForce(tVector force, tVector pos){
				getBody()->applyImpulse(FROMVECT(force), FROMVECT(pos));
			};
			
			void applyForce(const Force::tRef& force) {
				//attachedForces.insert(force); TODO
			};
			void applyImpulse(tVector impulse, tVector point) {
				getBody()->applyImpulse(FROMVECT(impulse), FROMVECT(point));
			};

			const tVector getVelocity() const {
				return TOVECT(getBody()->getLinearVelocity());
			};
			const tVector getAngularVelocity() const {
				return TOVECT(getBody()->getAngularVelocity());
			};
			const tVector getResultantForce() const {
				//return currentForce; TODO
			};
			tVector getAcceleration() const {
				// TODO
			};
			tScalar	getMassQuot() const{
				return getBody()->getInvMass();
			};

			bool hasPhysicalSystem() const{
				return false; // TODO
			};
			void setPhysicalSystem(PhysicalSystem* pSys){
				// TODO
			};
			PhysicalSystem*	getPhysicalSystem() const{
				return NULL; // TODO
			};

			void setActive() {
				getBody()->setActivationState(ACTIVE_TAG);
			};
		
			void setVelocity(const tVector& v) {
				getBody()->setLinearVelocity(FROMVECT(v));
			};
			void setAngularVelocity(const tVector& v) {
				getBody()->setAngularVelocity(FROMVECT(v));
			};
			void setAngularMomentum(const tVector& mom){
				
			};

			const PhysicalModel& getModel() const {
				return *mModel;
			};

			virtual unsigned CalculateSerializedSize();
			virtual unsigned Serialize(void * pBuf);
			virtual unsigned Deserialize(void * pBuf);

			/// Factory function
			static GameObject * CreateObject(const StringPool::Entry& sName);
	};

	/**
	 * Represents a non-movable object
	 **/
	class Static : public WorldObject
	{
		public:
			typedef GcReference<Static>	tRef;

			typedef struct StaticCreationOptions : public WorldObject::tCreationOptions {

			} tCreationOptions;

		public:
			Static(const tCreationOptions& args,
				   const WtfGraphics::GraphicsData::tCacheRef& d,
				   const PhysicalModel::tCacheRef& m);
			virtual ~Static();

			/// Factory function
			static GameObject * CreateObject(const StringPool::Entry& sName);
	};

	
	/**** Constraints: Allow construction of rigid body systems ****/
	
	class Constraint : public GcObject
	{
		protected:
			Constraint();
		public:
			virtual ~Constraint();
			
			GC_AUTOSIZE(Constraint);
	};
	
	class PointConstraint : public btPoint2PointConstraint, public Constraint
	{
		public:
			PointConstraint(const WorldObject::tRef& o1, const WorldObject::tRef& o2,
							const tPosition& p1, const tPosition& p2);
			
			GC_AUTOSIZE(PointConstraint);
	};
	
	class HingeConstraint : public btHingeConstraint, public Constraint
	{
		public:
			GC_AUTOSIZE(HingeConstraint);
	};
	
	class SocketConstraint :public btConeTwistConstraint, public Constraint
	{
		public:
			GC_AUTOSIZE(SocketConstraint);
	};
	
	class SliderConstraint : public btSliderConstraint, public Constraint
	{
		public:
			SliderConstraint(const WorldObject::tRef& o1, const WorldObject::tRef& o2,
							  const tPosition& p1, const tPosition& p2,
							  const tVector& minLinear, const tVector& maxLinear);
			
			GC_AUTOSIZE(SliderConstraint);
	};
	
	
	/**
	 * Fired when two objects collide.
	 **/
	class ObjectCollisionEvent : public ObjectInteractionEvent
	{
		public:
			ObjectCollisionEvent(GameObject * pObj, GameObject * pIntObj)
			: ObjectInteractionEvent(pObj, pIntObj) {};
	};
	
	class CollisionDetection : public Task<CollisionDetection>
	{
		public:
			CollisionDetection()
			: Task<CollisionDetection>(500, "CollisionDetection"){};
			~CollisionDetection() {};

		protected:
			void Run() {};
	};

	class PhysicsEngine : public Task<PhysicsEngine>
	{
	public:
		typedef std::list<WorldObject::tRef> tObjectList;
		typedef std::list<Constraint::tRef> tConstraintList;

		btDiscreteDynamicsWorld*	pWorld;
		btBroadphaseInterface*		pBroadphase;
		btCollisionDispatcher*		pDispatcher;
		
		tObjectList	mObjects;
		tConstraintList mConstraints;

		btScalar mTimeDelta;
		

		static void NearCallback(btBroadphasePair& collisionPair,
			btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo);

		public:
			PhysicsEngine();
			~PhysicsEngine();

			/// Returns the first object that the ray hits.
			WorldObject::tRef RayCast(const tRay& ray, tPosition& vHitPos);

			
			inline void PushObject(const WorldObject::tRef& pObj) {
				pWorld->addRigidBody(pObj->getBody());
				mObjects.push_back(pObj);
			};

			inline void RemoveObject(const WorldObject::tRef& rObj) {
				pWorld->removeRigidBody(rObj->getBody());
				mObjects.remove(rObj);
			};
			
			inline void AddConstraint(Constraint* p, btTypedConstraint* pBtConstraint) {
				pWorld->addConstraint(pBtConstraint, true);
				if(p) mConstraints.push_back(p);
			};

			const tObjectList& getObjects() const {
				return mObjects;
			};
			const tConstraintList& getConstraints() const {
				return mConstraints;
			};

			inline btDynamicsWorld * getWorld() const {
				return pWorld;
			};

			tScalar getRate() const {
				return 1.0 / mTimeDelta;
			};
			void setRate(tScalar rate) {
				mTimeDelta = 1.0 / rate;
			};

		protected:
			void Run();
	};
};


#endif

